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(54) Three dimensional position sensing system and method. 



A position sensing system calculates the X-Y-Z coordinates of a point (12) in space using triangulation 
and determines the orientation of an object located at that point (12). The triangulation calculation is 
based on the coordinates of at least three retroreflective elements (18) spaced apart from each other 
and the measured angles between the lines projected radially outward from the point (12) to each of the 
retroreflective elements (18). The accuracy of the measured angles is achieved by using a rotating 
member (30) supported by dedicated hardware and controlled by software. The member (30) rotates 
with a beam of light (16) generated by a light transmitting and detecting device (14) positionable at the 
point (12). The light transmitting and detecting device (14) receives the beam of light (16) reflected back 
from the retroreflective elements and generates an output signal in response thereto. A computer 
processes the output signals for use in calculating the X-Y-Z coordinates of the point (12) and the 
orientation of the light transmitting and detecting device (14) when it is positioned at the point (12). 
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The present invention relates generally to laser-based systems useful in construction applications and, 
more particularly, to a laser-based system and method for measuring the X-Y-Z coordinate of a point and the 
angular orientation of an object positioned at that point in a continuously changing frame of reference. 

In the construction industry, the level of automation and robotization is very low. One reason for this is the 

5 difficulty of properly positioning machines and tools. In the construction of commercial buildings, for example, 
various points of reference have to be established, such as placement of floors, ceilings, electrical wiring, 
plumbing, and heating and cooling ducts. Establishing reference points is time consuming and expensive, par- 
ticularly as such work is often contracted out to companies which specialize in this work. Moreover, for many 
applications, the machine has to move toward the product. When the reference point is continuously changing, 

10 the difficulty of positioning the machine is compounded. 

In prior laser-based systems, such as that disclosed in U.S. Patent No. 3,588,249, for example, a reference 
plane is established throughout a work site by a transmitter which emits laser energy in the level reference 
plane. The laser beam is projected radially outward from the transmitter and rotated continuously through 360 
degrees to sweep around the entire work site. One or more receivers may be employed throughout the work 

15 site to sense the location of this reference plane. Such receivers may be mounted on a surveyor's rod, as des- 
cribed in U.S. Patent No. 4,030,832, or they may be employed as part of a control system for construction or 
agricultural equipment, as described in U.S. Patent Nos. 3,813,171; 3,873,226; 3,997,071; and 4,034,490. 

I n order to track the movement of a reference point, prior art laser systems, such as the laser survey system 
disclosed in U.S. Patent No. 4,830,489, have provided not only elevation information, but also position infor- 

20 mation in two other axes. The system includes a laser transmitter, located at a reference position at a work 
site, which sweeps a laser beam radially in a reference plane. The system includes a receiver, located on mobile 
earthmoving equipment operating at the work site. The receiver has a sensor that determines the relative ele- 
vation of the laser reference plane. The receiver also includes a pair of reflectors, each of which reflects laser 
energy back to the transmitter. The laser transmitter also has a sensor which receives the reflected laser energy, 

25 and, in response thereto, produces receiver position information for transmission to the receiver. 

The laser transmitter is designed to rotate the laser beam continuously through 360 degrees at a substan- 
tially constant angular velocity and thus sweep the beam past the two reflectors of the receiver once during 
each revolution. During each revolution of the laser beam, the transmitter receives back two short bursts or 
pulses of laser energy from the two reflectors. Thus, since the laser beam sweeps at a substantially constant 

30 angular velocity and the distance between the reflectors is fixed, the time period between receipt of these two 
pulses provides an accurate basis for the calculation of the range or distance of the receiver from the transmitter. 
However, since the accuracy of the range calculation is dependent upon a uniform rotational velocity for the 
laser beam, any variability in the rotational velocity will decrease the accuracy of the range calculation. How- 
ever, this and other three-dimensional laser based systems are generally complex and expensive. 

35 It is seen then that there is a need for a positioning system and method wherein highly accurate X-Y-Z posi- 

tion coordinates and orientation of a point, which are insensitive to deviations in the rotational velocity of the 
rotating light beam, can be determined. 

This need is met by the three-dimensional position sensing system and method of the present invention 
wherein the X-Y-Z coordinates of a point can be calculated by triangulation. The triangulation calculation is 

40 based on the coordinates of at least three retroreflective elements spaced apart from each other, and the 
measured angles between the lines projected radially outward from a point located at an actual position of a 
light transmitting and detecting means to each of the retroreflective elements. One advantage of the present 
invention is that all of the active elements are in one location that is, at the light transmitting and detecting 
means, as the retroreflective elements are preferably passive elements. 

45 In accordance with one aspect of the present invention, a system for determining the position of a point in 

space comprises: at least three stationary retroreflective elements spaced apart from each other and stationed 
at known coordinates, the retroreflective elements capable of reflecting light back toward a light source; light 
transmitting and detecting means, positionable at the point, for generating a rotating beam of light to illuminate 
the stationary retroreflective elements during rotation, and for detecting the beam of light when it is reflected 

so from the stationary retroreflective elements and generating an output signal in response thereto; and computer 
means responsive to the output signal for computing, from the known coordinates of the retroreflective elements 
and from the angular orientation of the beam when the beam illuminates the retroreflective elements, the coor- 
dinates of the position of the point in space. 

In a preferred embodiment of the system, the light transmitting and detecting means comprises: means for 

55 generating a beam of light; means for projecting the beam of light at a substantially constant rotating angular 
velocity toward the retroreflective elements; means for receiving the beam of light reflected from the retroref- 
lective elements corresponding to the illumination of the retroreflective elements during rotation of the beam; 
and azimuth and zenith means, responsive to reflection of the beam of light from the retroreflective elements, 
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for continuously transmitting angle signals indicative of an azimuth and zenith angle at which the means for 
receiving the beam of light is positioned with respect to each of the retroreflective elements. 

In accordance with one aspect of the system for determining the position of a point, the stationary retroref- 
lective elements have a distinctive retroreflective design, such as a slant target extending substantially diagon- 

5 ally from one top comer to the opposing bottom corner of the retroreflective elements in combination with a 
retroreflective bar code. In a structured environment, then, the computer means can designate a number for 
each retroreflective element in a rotation, and the number will remain the same for each rotation. 

In a further aspect of the present invention, the light transmitting and detecting means includes a member 
rotating with the beam, the member having a periphery and further having a plurality of angularly positioned 

10 elements spaced around the periphery which divide a revolution of the member into a plurality of generally equal 
partial revolutions; and a means for detecting movement of each of the elements past a predetermined point 
as the member rotates. Preferably, the plurality of angularly positioned elements comprises a plurality of aper- 
tures and the means for detecting movement of each of the elements comprises a light source paired with a 
photodetector element A further preferred embodiment includes means for registering a tilt position of one or 

15 more reflecting mirrors and means for calculating a zenith angle of the beam 16 based on the tilt of each of the 
minors. 

The present invention also provides a method for determining the position of a point in space. The method 
comprises the steps of: locating at least three stationary retroreflective elements spaced apart from each other 
and stationed at known coordinates, the retroreflective elements capable of reflecting light back toward a light 
20 source; transmitting a rotating beam of light to illuminate the stationary retroreflective elements during rotation; 
calculating an angle between the direction of the beam of light when the beam illuminates each of the retroref- 
lective elements; tracking scan behavior; using the scan behavior to calculate angular measurements; and 
determining the position of the point based on the angular measurements and the coordinates of the retroref- 
lective elements. 

25 In a preferred embodiment of the method for determining the position of the point, the step of tracking scan 

behavior further includes the steps of mounting a member on a rotating shaft, the member having a periphery 
and further having a plurality of apertures located at the periphery which divide a revolution of the member into 
a plurality of generally equal partial revolutions; and registering a time differential between each adjacent pair 
of the plurality of apertures as the member rotates. Also, the method includes the step of interpolating between 

30 corresponding angle measurements of an adjacent pair of the plurality of apertures to determine exact angles 
between each pair of apertures. 

In a further embodiment, the step of tracking the scan behavior includes not only the determination of the 
X-Y position of the point, as described above, but also the determination of the Z position of the point. This can 
be accomplished by registering the tilt position of one or more reflecting mirrors and calculating the zenith angle 

35 of the exit beam based on the registered mirror position. 

Accordingly, it is an object of this invention to provide a position sensing system and method, wherein the 
X-Y-Z coordinates of a point can be calculated by triangulation. It.is also an object of this invention to provide 
such a system and method wherein the orientation of the system when it is located at the point can be deter- 
mined. It is a further object of this invention to provide such a system wherein the calculations are insensitive 

40 to deviations in the rotational velocity of the rotating light beam. Finally, it is an object of this invention to provide 
such a system which utilizes software to calibrate a code wheel or other member to improve the accuracy of 
the calculations. 

Other objects and advantages of the invention will be apparent from the following description, the accom- 
panying drawings and the appended claims. 
45 In order that the invention may be more readily understood, reference will now be made to the accompany- 

ing drawings, in which: 

Fig. 1 is an isometric view of the position sensing system of the present invention located at a pointwithin 
a structured environment defined by four intersecting walls; 

Fig. 2 is a side view, with parts broken away, of a light transmitting and detecting means of the position 
so sensing system of Fig. 1; 

Fig. 3 is an exploded partial top view of a rotating member illustrated in Fig. 2; 

Fig. 4 is a calibrating system which is used in conjunction with related software to calibrate the light trans- 
mitting and detecting means of Fig. 2 which includes the member of Fig. 3; 

Fig. 5 is a schematic block diagram of a hardware interface, controlled by software, which supports the 
55 position sensing system of the present invention, illustrating the interrelationship between the retroreflec- 

tive elements shown in Fig. 1 and the light transmitting and detecting means shown in Fig. 2; and 
Figs. 6A and 6B illustrate a flow chart detailing the steps used in determining the Z-position of the point in 
space shown in Fig. 1. 
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The three-dimensional position sensing system and method of the present invention is advantageously 
utilized in either a structured environment 10, as shown in Fig. 1 , or an external environment, to generate con- 
stantly updated information on the exact location of a point 12. Typically, light transmitting and detecting means 
14, best illustrated in Fig. 2, is positioned at the point 12 for transmitting a rotating beam of light 1 6, which is 

5 emitted by a light source such as laser 20 of Fig. 2, and reflected back to the light transmitting and detecting 
means 14 by a series of retroreflective elements 18. The position sensing system is also capable of determining 
the orientation of the light transmitting and detecting means 14 which is positioned at the point 12. The rotating 
beam of light 16 should be of a size appropriate to create discernible START signals and END signals from 
each of the retroreflective elements 1 8, which preferably consist of distinctive retroreflective bar code elements 

10 in the form of a diagonal slant target from a top comer of the retroreflective element 1 8 to the opposing bottom 
corner in combination with a retroreflective barcode, as illustrated in Figs. 1 and 5. 

The position sensing system has at least three stationary retroreflective elements 18, as typified by ele- 
ments 18a, 18b, and 18c in Fig. 1. Preferably, the retroreflective elements 18a, 18b, and 18c are spaced apart 
from each other around the periphery of the frame of reference and stationed at known coordinates in the frame 

15 of reference 10. In a preferred embodiment of the present invention, the retroreflective elements 18 are passive 
sensors, although active sensors may be used as well. With the passive sensor elements 18, the beam of light 
16 is reflected back to the light transmitting and detecting means 14. The beam 16 transmitted by the light trans- 
mitting and detecting means 14 has a varying zenith angle such that a dynamic scan pattern 17 of the beam 
16 can scan the environment 10 at varying elevations to strike the retroreflective elements 18, regardless of 

20 the elevation of the elements 18. Azimuth and zenith means (not shown), responsive to the reflection of the 
beam of light 16 from the retroreflective elements 18, continuously transmit angle signals, indicative of an 
azimuth and zenith angle at which the light transmitting and detecting means for receiving the beam of light is 
positioned with respect to each of the retroreflective elements 1 8, to a computer 22 of Fig. 5. The computer 22 
includes a microprocessor, such as a Motorola 68030, having memory means for providing storage of software 

25 and data. A listing of exemplary software for performing the angle measurements in accordance with the present 
invention is as follows. 
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G -otAng I es ( ) 

Struct trans *t : 
Struct angle *a ; 

register int ia, na, itr, isensor; 

itr =0; 
t = T ; 

whi1e( itr < Ntrans ) { 

if( isensor-HefWndw(t) ) < 

if( (isensor>0) && ( isensor<MAX_SENSOR) 
Aave[issn£or] « ARefCt); 
Nave [isensor ] + == 1 ; 
itr 8 ; t += 8; 

} 

> 

else { 

it:r++; t++: 

> 



if( ( Ticks-Pticks) < 200 ) return(l); 
Pticks = Ticks; 



e» = A; 

a->a M_2Pl ; 

a->isensor = 0; 

na = 1 7 

a++; 

t'or( ia=l ; ia<MAX_ANGLE; ia++ ) { 
ifC N*ve[ia] ) { 

a— > isensor - ia; 

a— >a = AaveCla]/Nave[iaJ ; 

a++: • na++; 

> 

AaveLiaj = 0.0; 

Nave[iaJ = 0; 

> 

i*( RqAngle »» -1 ) { 

forC la=0; ia<n«; 1a++ ) copALia] - ACiaJ; 
RqAngle = na ; 

} 



1 



return( na ); 



Get'frans( ) 

( 

struct event *© ; 
struct trans *t : 
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register int ntn, iev ; 

register int icw, stime, ctime; 



ntr 
iev 



= 0 

= 0 

= E 

= I 



10 



15 



20 



while( 1ev<Nevent > ( 

if( !(e->icw&CW) > { 
e++; iev++ ; 

> 

el se { 



icw = e->icw&CWHL MASK; 

stime = e-> t i me& T I ME_MASK ; 
ctime = Tcw( icw+1 >; 

t— >a = Cang1e[icw3 + Cw Inter ( st ime) ; 
ifC l(e->1cw&R_START) ) t->type = RESTART; 
el se 

-ffC !(b->Tcw&R_END) ) t->typB = R_ENO; 
else t-> type « -1 ; 



25 



30 



35 



) 



e++; 1 BV++ ; 
t++; ntP++; 



> 



-T-f( RqTrans — -1 ) { 

for( i=0; i<ntr; > copTCi] 

RqTrans = ntr; 
if ( RqTrans == 0 ) { 
RqAngle = 0 ; 

) 

} 

return( ntr ); 



T[1i; 



6etEvent() 
register int i ; 



1f< Rq Event ~ -1 ) < 

for< i=0 ; KNevent ? i + + ) copE[1] = EL*U ; 
Rq Event ~ N event ; 
ii=C RqEvent mm o ) { 

RqTrans =0; 

RqAngTe = 0; 

} 

> 

return( Nevent ) ; 



50 



IntrC) 
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n t V'ifo; 

register int i . icw; 
register struct sf *s = SF- ; 

do { 

if( (Nevent> 1 U23) )| ! (s->sta cuc&KULL ; ) { 
pri net ("Reset f=if oi ! ! \n ' ) ; 
FifoReset(); 

> 

fifo - s->data; 

icw = fifo & CWHL_MASK; 

if ( Mew ) { 

if( En Time ) Stime(U): 

6etPos( &CP ); 

pe = £; 

Nevent = 0 ; 

> 

else { 

*pe++ = *(struct event *)(&tifo): 
Nevent*-*-; 

> 

} 

whi1e( s->status&BMPTY >; 

static float ARef( t ) 
Struct trans t£] ; 

float a; 

a = CtL03-a + trL'H-a) / 2.0; 

return( a ); 

i'cw( icw ) 
register int few; 

register int i, xcw; 

fori *=0; KNevent; i++ ) ( 

if( EMJ.icw&CW ) continue; 
xcw = G [ i ] . i cw&CWH L_MASK ; 

i-f( xcw == icw > return( E ti ] . t i me&T 1ME MASK ) 

> 

return (— 1 > ; 

) 

f/ifoResetO 
1 
i 

register int i; 

register struct sf *s = SF; 

Nevent = 0; 
Ntrans = 0; 
Nang'le =0; 
RqEvent « 0; 
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RqT rans 
RqAngl e 
pe 



= 0; 
- 0; 



Pticks 



= Tic ks ; 

= s->reset; 



for( i=0; i <MAX_AN(5LE; i + + ) { 
Aave[i] = 0.0- 
Nave[i] = O; 



Since the reflection from one passive element 18 looks the same as the reflection from each of the other 
passive elements 18, it cannot be determined from the reflection itself which element 18 is being illuminated. 
Therefore, according to one aspect of the present invention, any or all of the retroreflective elements 1 8a, 1 8b, 
and 18c can have a unique barcode in order to differentiate one element from the others. The moment of illumi- 
nation of each retroreflective element 18 is registered extremely accurately since that moment is decisive for 
determining the angle within the angle of rotation of 360 degrees. By recognizing the retroreflective elements 
18a, 18b, and 18c individually, it is possible to assign the proper coordinates to each element 18, which is neces- 
sary for the calculation of the position of the point and the determination of the orientation of the light transmitting 
and detecting means 1 4 located at the point 12. Assigning a unique bar code to each element 1 8 distinguishes 
each of the elements 18a, 18b, and 18c from the others. The computer 22 includes a microprocessor, such as 
a Motorola 68030, having memory means for providing storage of software and data. A listing of exemplary 
software for performing the bar code recognition of the present invention is as follows. 



9 



EP 0 468 677 A2 



static float fdarposl j 



RefWndw( t ) 
Struct trans tCJ; 
{ 

float diff. tdiff; 
r eg ister int 1 , itr; 
float as, ae, aU r al; 



i 0 . lt>. O . 23 . 

U . 4 7 . 



U.41 . 
0 . t>5 



if( 1 (t tOj -rype&RSTARl ) ) return(O); 
tdiff = ttO]-a - t[7],a; 

Tf( tdiff > 0.114 ) return(O); 

if< ! (t£l] .type&R_EN0) > return(O); 

it( I (tL2] .type&R_START) > return(O); 

if( ■ <t[3] .type&R_EN0) ) return (0 ) ; 

if( ! CtL4] - type&R_START) ) return(O); 

ifC 1 (t [S] .type&R_EN0) ) rqturn(O); 

">f( I (t[6] .trype&R START) > peturn(O); 

if( I (t£7j .type&R~ENO) ) return(O); 



tdiff « 1 -O / tdiff j 

as = (t[0] ,a-tt2l .a> * tdiff; 

ae =* Ctt&] -a-tt73 .a) * tdiff; 

if( (as<0.12) 1| <as>0.18) ) return(O); 
if( (ae<0.12) |j <ae>0.18) > return(O); 

tdiff = 1.00 / (tCZJ .a-ttS] -a) ; 

dlff = (t[2].a - (tt3].a+tL4l-a>/2.0) * tdiff; 

for( i=1 ; i<9; i++ > { 

if( (d1ff >8arPos[i-11) && (dif f <Ba r Pos fi] ) ) return(i); 

> 

peturn(O) ; 

> 

static float ARef( t ) 

struct trans tQ; 

< 

float a; 

a = (tLOJ.a + tl'H.a) / 2.0; 
return( a > ; 

> 

Tcw( icw ) 
register int icw; 
( 

register* int i . kcw : 



for( i=0; KNevent; i-«- + ) { 

if( e[*iJ-TCW&CW ) continue; 
xcw = E[i] . icw&CWHL__MASK; 

if( xcw — icw ) return ( E [ i j . t ime&T IME_MASK ); 

} 

return (—1 ) ; 



Using the angle measurements and the recognized barcode patterns, the X-Y coordinates of the point 12 
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can be calculated by triangulation, and the Z coordinate is determined in accord with the flow chart of Figs. 6A 
and 6B. The computer 22 includes a microprocessor, such as a Motorola 68030, having memory means for 
providing storage of software and data. A listing of exemplary software for performing the triangulation calcu- 
lations of the present invention is as follows. 



"include 'cap. fi- 

tt ndetine Cwln t er(x) ( x*(Cang l c L icw+ l J -Cong le( icwj ;/ c tinuO: 

t loat A«et() ; 
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int NQvent, Ntrans, Nangle; 
static int RqEvent, RqTrans, KqAngle; 

struct event E [MAX_EVEN T ] , copE [MAX_E VBN T ] 

struct trans T CMAX_T RANS J . copT £MAX_TKANS J 

struct angle A CMAX_ANGLE] . copA [MAX^ANGLEj 

struct event *pe ; 
20 struct Co *Sp; 

static int Ptlcks, Nave £MAX_ANGLEj ; 
static float Aave£MAX_ANGLE3 ; 

struct Curf>os CP; 
25 rioat x[MAX_ANGLEj, y [MAX_ANGLE3 ; 

static float BarPosC] = ( 0.16, 0.23. U.29, 0.41, 

0.47, U.&3, U.!>9 r O.tib }; 

GetPos( cp ) 
30 register struct CurPos *cp; 

struct Co P; 
struct Mc ml r m2 ; 
register int 1. sr. ipref? 
float xpref. ypref, alfa; 
35 register struct angle ^pl r *p2 ; 

Nevent = GetEvent(); 
Ntrans = Get Trans ( ) ; 
Nangle « GetAnglesQ; 

40 -Tf< Nangle < 4 > return( C >; 

ifC EnTlme ) NxtTimeO ; 



p1 = 

p2 = &AC2]; 

m_coord( p1->isensor, p2->1sensor. p2->a-pl->a . &m2 ) - 
pl++; p2++; 

Tpref Ppref ( M_PI_2, 0); 

for( 1=1; KNangle-2; i+*, p1++, p2 + + ) ( 

ml = m2; 

m_coord( p1->1sensor, p2->isensor, p2->a-pi->a, &m2 ); 
alfa = sq( &m1 „ &m2 , p1->1sensor, &P ) ; 

ipref = Ppref( alfa. 1 ); 
XC1] = P.K; ytij = P.y ; 
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cp->ipret = ipret; 

cp->x - x(.iprst j ; 

cp->y = y[ ipref J ; 

cp->ang1e = CapAng'le( cp ); 

cp->time = Ticks: 
cp->nca++ ; 

if( EnTlme ) NxtTi'me(); 

} 

n_coord< sa, sb, olfa, mc ) 
register int sa „ sb; 
float alfa; 
struct Mc *mc; 
I 

float dx, dy; 

float sinalfa, a"lfa2 e k„ beta, teta ; 

dx = S [sb] .x - S[saj .x; 
dy = S£sb3-y - SCsa} .y; 

" k = sqrt( dx * dx + dy * dy) ; 

if( sinolfa=*s1n(a1fa) ) mc->r = 0.5 * k / sinalfa: 
else mc~>r « 0.0; 

beta = xatan( dx r dy ); 
alfa2= M Pl 2 ~ alfa? 
teta = aTfa2 + beta; 

mc->x « S[sa].x + mc->r * cos(teta); 
mc->y = S [sa J .y + mc->r * sln(teta); 

> 

float sq ( ml, m2, sm, l> ) 
struct. Mc *m1 , *ra2; 
register int sin; 
struct Co *P; 
{ 

float x1, yl ; 

float x2. y2; 

float Tx f Ty, Tc, Tr; 

float a1, a2; 

x \ — m 1 — >x ; y1 — ml— >y; 
K2 = m2->x ; y2 = m2->y; 

/* 

(x - xi )~2 + (y-yl )~2 = rl*2 
(x - x2)"2 + (y-y2)~2 = r2~2 

x~2 - 2xx1 + xl~2 + y~2 -2yyl + y1~2 = r1~2 
x"2 - 2xx2 +■ x2 A 2 + y~2 -2yy2 4- y 2^2 — r2"2 



2xx2 -2xx1 -x2~2 + x1~2 + 2yy2 -2yy1 - y2~2 + y 1 "2 = r1~2 - r2~2 
*/ 

Tx = 2.U * X2 - 2.0 * XI; 
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Jc « "(x2 * x2 J + (xl *'xi; - < y2 * y 2) * (yl - yl) : 
fr = (ml^r * n,1->r) - (m2->r * \n2->r ); 

Tx = -lx / i y: 

f y > = (Tr - Tc ) / fy; 

else Tc - O.O- 

/* 

x~2 - 2xx1 + x1~2 y~2 -2yy1 + yl~ 2 = r r 2 

x^2 - 2XX1 + xr2 + (Tx+Tc)^ - 2(Tx+Tc)yl + yl'2 = r1~2 

X 2 ~ 2xx1 + K1-2 4- (Tx~2x ^2TxTcx +Tc~2) - 2(Tx + Tc)y1 + yl 



a = 1.0 (Tx * Tx ); 

b = -2.0*x1 + 2.0 * Tx * Tc -2.0 * Tx * yl ; 

c = xl * xl + Tc * Tc -2.0 * Tc * yl + yl * yl ; 

c = c - (ml ->r * ml ->r ) ; 

P->x = -b / a - SCsmj.x; 
p->y = Tc + Tx * P->X; 

al = xatan ( m1->x-P~>x, m1->y-P->y ) ; 
«2 = xatan( m2->x-P->x, m2->y-P->y > ; 
return( fabs(al-a2) ); 

float xatan( dx # dy ) 

float dx, dy; 

{ 

float x; 



/* 
*/ 



ff< (dx >« 0.0) &5c (dy >= 0.0) ) { 
if( dx « O.fl ) x = M_PI_2; 
e1se x « atan( dy / dx ) ; 

e l se 

if( (dx < 0.0) (dy >= 0.0) ) { 

x = M_Pl - atan( dy / -dx ); 

el se 

if( (dx < 0.0) && (dy < 0.0) ) < 

x = M_PI -t- atan( -dy / -dx ); 

el se 

if( (dx >= O.O) && (dy < 0.0) ) 
{ 

if(dx~0.0)x= 3*M_P1 2; 
j else x = 2*M_Pr- atan( -dy / dx ) 

return ( x ); 



float C«pAngle( cp ) 
struct CurPos *cp; 
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20 



30 



f I oa C dx, dy , a ; 
register int ipret , 
register struct Co 



ipret - cp-> ipret; 

i = A I ipret J . i sensor ; 

s = Sp + i ; 

dx = s - >x - cp->x; 

dy = s ->y — cp- >y ; 

a - xatan( dx, dy ) - A(ipretJ .a - A_INOEX; 
TfC a > M2P1 ) a -= M2P1; 
it( a < 0.0 ) a += M_2Pi; 
return (a ) ; 



15 >pret( alfa f i ) 

f loac a 1 1 a ; 
register int i ; 
( 

f I oat temp; 
Static int ipref; 
Static float Apref ; 



1¥( !i ) { 

i pref = i ; 
Apref = alfa; 
25 return(i); 

> 

if( alfa < 0.0 ) return(ippef); 
whiTeC alfa > M_2Pt > alfa -= M_2Plj 
if( alfa < 0.0 J return(ipraf ); 

wh1*le( alfa > M_PI ) alfa = M_ 2PI - alfa: 



temp=f absCalf a-M_PI_2); 
if( temp < Apref ) { 

5 pref =1; 

Apref = temp; 

> 

return( ipref ); 



40 Referring now to Fig. 2, a side view of a housing 24 for the light transmitting and detecting means 14 is 

shown. The light transmitting and detecting means 14 includes an electric motor 26 mounted to rotate shaft 
28. A member 30, such as a code wheel, and a light diverting mirror 32 are mounted on the shaft 28. A mirror 
positioning means 33 provides movement or positioning of the mirror 32. The tilt of the mirror 32 is known for 
a current position of the member 30 such that the vertical height of the retroreflective elements 18 are always 

45 known with respect to any horizontal point. 

In the present invention, the beam 16 has a variable zenith angle, which can be varied by changing the 
position of one or more mirrors 32, 35, and 37, shown in Fig. 2. For instance, the main mirror 32 can be tilted 
using spring 39 which cooperates with the mirror positioning means 33. Alternatively, mirror 35 can be tilted in 
two dimensions in order to properly project beam 16 onto mirror 32. Yet another means for varying the zenith 

so angle of the beam 16 is to tilt mirror 37 independently or in conjunction with mirror 35. Mirror 37 is optional, 
however, as the laser 20 may direct beam 16 straight at mirror 35. 

An index wheel 34 can also be included for providing a single reference pulse indicating each complete 
rotation of the shaft 28. A light source, such as solid state laser 20, directs the beam of light 16 onto the rotating 
mirror 32 so that a plane of rotation is created. In a preferred embodiment of the invention, a generally horizontal 

55 plane of rotation is created. However, any plane of rotation, including vertical, can be created. In addition, the 
beam 1 6 has a zenith angle which may be varied to produce a non-planar scan pattern. The retroreflective ele- 
ments 18 are positioned in this plane or scan pattern. An advantage of the present invention is that a precisely 
horizontal or vertical plane or scan pattern is not essential for the three dimensional position calculation. As a 
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result, the system may be used either by an operator as a handheld device as the operator moves about the 
environment of interest, or mounted on vehicles and other equipment as the equipment moves about a given 
area. 

When the rotating laser beam 16 strikes the retro reflective elements 1 8 during each revolution of the shaft 
28, the beam of light 16 is reflected back to the light transmitting and detecting means 14 and can be transformed 
into an analog signal by a suitable detector, such as photodetector 36, and transmitted to a signal processing 
means 38 of Fig. 5, which outputs two digital signals. 

Theoretically, the moment in time that a retroreflective element 18 has been illuminated with respect to the 
complete time of rotation is related to its angle, depending on which retroreflective element 18 is being illumi- 
nated by the light beam 16, within a total angle of 360 degrees. However, this is only the case if the rotational 
speed of the beam 16 is extremely constant. Typically, the rotational speed of motor shaft 28 which causes 
the beam 1 6 to rotate is not perfectly constant and it is not possible to obtain a constant speed with the accuracy 
which is desired by the position sensing system of the present invention, especially in a mobile operation. Con- 
sequently, the position sensing system utilizes the motor 26 in conjunction with the member 30, supported by 
the dedicated hardware interface of Fig. 5 and controlled by software to achieve the desired accuracy. The com- 
puter 22 , having a microprocessor such as a Motorola 68030, contains software for a main routine which con- 
trols the hardware interface of the position sensing system, an exemplary listing of which is as follows. 
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10 



15 



Sine 1 U cJ e 
3 i nc I ude 
Bine I ude 
a i nc 1 ude 



<stdio.h> 
<set jmp . h> 
<rnath . h> 
'cap.h" 



jmpbuf env_ctr'lc; 

static int SwK'lags; 

mt Ndrws , OspClalm; 

int PTicks, PKticks; 

int Toggle, Xzoom, MMmode ; 



float Ralfa; 

float x[MAX_ANGLE] , 



y[MAX — ANGLE] , 



char LineC^O], pLin©L30j; 

int Timer() , intrlndex() r Intrlr(); 



20 



struct CurPos TmpCp , PrvCp; 

char Header LJ - "\n**** CAPSY ****\ r \n* 

char buffer [.80], cmd£6Uj, largsC»0D; 

inTT-Stdin, Stdout; 



25 



30 



40 



45 



50 



main ( ) 
( 

int 1 ; 



Stdin = Stdout =0; 

1f( TstOefQ > Set0efau1t< 0, 1 )j 



gen_coord(U) ; 
gen_cangle(0) ; 



Ticks 

En Time = 

CP .x 

CP.nca - 
CP.angle= 



0; 
0; 
0; 
0; 

0; 



PTicks 
OspC 1 aim 
CP.y 
CP . pnea 
Ralfa 



= 0; Toggle - 0 

= 1 ; Xzoom *= 0 

« I); CP. time = 0 

= 0; MM mode = 0 
= 0; 



for(i=t); i<2S; L1neti++] = 0 ) ; 
for(i=»; i<2S;pL1neLi++] = 0 ); 

I2cln1t(); 
K1foReset(); 

SetVector( AVEC6 , Timer ); 
SetVector( AVEC5 , Intr Index ); 
SetVectorC AVEC3 r Intrir ); 
SetLvl ( LVL2 ); 

Oelay< 1.0 ); 



SetPtty( TERM ); 
fprlntfC Stdout, 



Header >; 
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fprinct-(. o, "ley co 3CdU*d.\n~, Scdin ) 
fpr-inCfC 1. "ICy to S td W^d . \n " , Stdin ), 

if( setjmpl envcCrlc ) > { 
SetPtty( GRAPHICS ); 
DrwMob( -Nmob ); 

Nmob = Ncoord( Mob, MAX_M08 ); 
10 DrwMarkO; 
> 

SetPtty( GRAPHICS >; 



15 



> 



DspCla1m= 0 ; 
MMmode = 0 ; 

TtyControl(); 



20 TtyControlO 

register char c; 
register 1nt 1=0; 



25 



do { 

c = -fgetc(Stdin) ; 

1f( c & CTRL_CHAR ) CtrlControl( c ); 

> 

while( c != •** ) ; 



3° DspCtalm = 1; 

SetHtty( TERM ); 



for(;;) { 



35 



40 



fprintf( Stdout, "Enter command:"); 
1f( !getcmd( cmd „ largs ) ) c = 0; 

else c ** cmd{.03; 

fprlntfC Stdout , "\n*)s 

AE_cmd( c ); 

sw1tch( c ) { 

case 4 2* : 

PeFaul C() ; 

break; 

case '3 1 t 

AeCrndO; 

break; 



50 case '4' 



SwapTtyQ : 

brea k; 
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case b * 

break ; 
case ' 6 
break ; 
case * 1 

case * 9 1 



Mao Move (. ) ; 
l ongjmp( env_ctrlc 



ManSca le( ; ; 



Update(>; 



SetPtty( GRAPHICS 
o pen _g fx() ; 



case D 
case '0' 

break ; 



"longjmp( env_ctr Ic 



> 



Ctr1Control< c ) 
register char c; 
< 

register char cO-c; 



0s pCl aim = 1 ; 

c &- Ux3f; 
sw(tch< c ) { 



case * : 
case 1 0 * : 
case ' \r ' : 

ZoomCmd ( c ) ; 

CU = -1; 

break; 



case ' 1 ' 
case *3' 

break; 
case '9' 



DlrectMarkC c ); 
SetPtty( GRAPHICS ); 
SwapGfx( Xzoom ); 
CO = -1 ; 



break ; 



SetPtty( GRAPHICS ); 
SwapGfx( Xzoom ); 
CO ■ -1 ; 
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10 



15 



25 



35 



45 



V s pC lai'm - O ; 

return ( cO ) ; 



AeCmdC ) 
i 



fprintf( Stdout , " 1 = tvents\n - ) ; 
tprintt ( Stdout, " 2 = Ang 1 es\n * ) ; 
f pn'ntf ( Stdout, "Select type : "?"); 

geccmd( cmd, largs ); fpn'ntfC Stdout, "\n"); 

if( cmd[0] == • 1 ' ) cmd[U] = * E* ; 

ifC cmd[0] == '2' ) cmd [a] = 'A'; 



Atf_cmd( cmdCOj. ) ; 
> 

Swap Tty ( ) 
{ 

if( stdm == o ) { 

20 - Stdin « Stdout = 1 ; 

} 

else 

if( Stdout 1 ) { 

Stdin = Stdout = 0; 

> 

fprlntfC 0, "Tty swap to Std»%d . \n" . Stdin ); 
fpr1ntf( 1, "Tty swap to StdB%d-\n", Stdin ); 

> 

ManMove( ) 
{ 

30 register char c r cO; 

register int step; 
struct CurPos cp; 



SetPtty( GRAPHICS ); 
0spClaim= 0; 
MMmode = 1 ; 

for( ;? ) { 

cp = CP; 

i-f( iXmove(&cp) ) return; 
CP = cp; 

> 

} 

ManSca le( ) 
{ 

register char c„ cO; 
register int step; 
struct CurPos cp; 



SetPtty( GRAPHICS >; 
UspCla1m= 0; 
50 MMmode = 1 ; 

-for(;;> { 
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Xmove(. &cp ) : 
CP = cp; 

) 

) 

<move( cp ) 

register struct CurPos *cp; 

register int step; 
register char c, cO ; 

c = cU = f getc( Std in ) ; 

step = 100; 

if< c & CTKL_CHAR ) ( 

c - CtrlControl( c ); 

C &- Ox'Jf; 

step *«= S ; 

) 

switch( c ) { 



case *6* : cp->x step; break; 

case : cp->x += -seep: break; 

case '2' : cp->y ■#■■ step; break; 

case *8* : cp— >y += -step; break; 

case '5* = 

if( cO&CTRL_CHAK ) cp->angle -~ degrad(lS-O) 

else cp->angle +» degradOS.Q) 

whi1e( cp->angle > M_2P1 ) cp->angle -= M_2PI ; 
wh1le( cp->angle < 0.0 > cp->ang1e +- M_2PI ; 

break; 

case 0 s 
case * \n ' z 
case ' \r * z 

return 0; 

break; 



} 

cp->nca++; 
return 1 ; 

) 

ZoomCmd ( c ) 
register char c; 
( 

if( c = = ' * ' ) Xzoom ; 

else 

i-f( c == ' \r ■ ) Xzoom++ ; 
el se 

ifC c == '0* ) Xzoom = 0; 

1f( Xzoom < 0 ) Xzoom = 0; 
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if< Xroom > 10 ) Xzoom = io- 

Swapt5tx(. Xzoom ) t 

5 > 

01rectMarK( c ) 
register char c; 
t 

register inc i ; 
10 struct CurPos cp; 

Cp = CP; 

1 f ( c =- M ' ) { 

OrwMobC -Nmob ); 
KrzMob( &cp. Mob, Mrk ); 
15 Nmrk = Ncoord( Mrk, MAX MRK ); 

DrwMark(); 

> 

else 

1f< c ss - 3 ' ) < 

OrwMob( -Nmob ); 

for( 1=0; 1 <MAX_MRK ; i ) Mr k Li 3.x = ftrkLi3.y - -1.0; 

Nmrk = Ncoord< Mrk, MAX_MRK >; 
DrwMark(); 

> 

> 

Intr Ticks( > 
{ 

T-icks++; 

1f< IDspClaim <T1cks-PTicks >>«<f=_CLOCK/A ) ) { 
TmpCp = CP; 
g"fx_point< fitTmpCp ); 
PTIcks = Ticks; 

> 

> 

9"fx - po1nt(cp ) 
register struct CurPos. *cp; 
< 

register int 1 ; 

/* if( (cp->nca==PrvCp.nca> && iMMmode ) { */ 
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25 



30 



1f( (cp->nca~PpvCp.nca) ) { 
M 1fC Toggle 1 > { 

OrwMobC -Nmob ) ; 
return; 

> 

> 

PrvCp = *cp; 

45 cp->angle *= Ralta-, 

i*( tMMraode ) sprlntfC Line, "Xs%S.0f Yi%S.0f Oritb.lf", 
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cp->x, cp->y. raddGg(cp->angie) ); 
else spnntt( Line. "x:%S.Ot y:%b.Ot 0-%b.\t". 

cp->x, cp->y, raddeg(cp->angle) >; 

Line£2b] = O; 

Wr L 1 neC Line ) ; 

UrwMob( Nmob, cp, Mob ; ; 

} 

WrLine( line ) 
register char * 1 1ne ; 
{ 

register int i # x; 
register char *p , * 1 ; 

p = pL1ne; 1 = "line; 

wh1le( *1 ) { 

if( *! 1= *p ) { 

wMle( *1 ) *p+ + = 

wstring( 6 r 119 r line ); 
return ; 

> 

> 

> 



As shown in Fig. 3, the member or code wheel 30 has a plurality of angularly positioned elements, preferably 
apertures 42, spaced around its periphery which divide a revolution of the member 30 into a plurality of generally 
equal partial revolutions. The size and spacing of these apertures are greatly exaggerated in the drawing for 
clarity of illustration. For example, the code wheel 30 may divide a revolution into one thousand generally equal 
parts positioned approximately 0.36 degrees apart by spacing one thousand elements or apertures 42 around 
the periphery of the member 30. Although the distance between each adjacent pair of apertures 42 theoretically 
represents a movement of 0.36 degrees, misalignment of the member 30, misalignment of the center of the 
member 30 through which the shaft 28 extends, and manufacturing tolerances, cause deviations in the spacing 
of the elements 42. However, since these deviations remain constant once the light transmitting and detecting 
means 14 is assembled, the actual angular spacing between each element 42 in the member 30 can be deter- 
mined extremely precisely by a software calibration table. 

Calibration of the code wheel or member 30, preferably using software, improves accuracy by eliminating 
errors due to misalignments, deviations, and irregularities of the rotational speed of the motor 26. Any speed 
fluctuation of the motor 26 between two apertures 42, particularly when there are one thousand such apertures 
spaced around the periphery of the member 30, will be negligible. Consequently, it is possible to interpolate 
between an adjacent pair of apertures, such as 42a and 42b in Fig. 3, to determine an exact angle between 
the pair of apertures 42a and 42b, according to the equation: 



Angle = Z42a + (Tm/Tcw) * (Z42b - Z42a) 



where ^'42a is the measured angle of aperture 42a; L 42b is the measured angle of aperture 42b; Tm is the 
time elapsed between passage of the previous aperture, here aperture 42a, and the moment M in time that the 
reflecting light strikes the sensor or photodetector 36; and Tew is the time it takes the code wheel 30 to move 
between element 42a and element 42b. 

One method of calibrating the code wheel or member 30 is illustrated in Fig. 4 as a calibration system 44. 
To calibrate the member 30, the light transmitting and detecting means 14 containing the code wheel 30, is 
placed on a device 46 having an angularly positioned rotating surface 48, with the member 30 arbitrarily posh 
tioned. The computer 22 of Fig. 5 controls a stepper motor 50 associated with the surface 48 and determines 
the number of steps between each pair of adjacent elements 42 as the member 30 rotates. For instance, if one 
revolution of the member is defined as 360,000 steps, and the member 30 contains 1000 generally equally 
spaced elements 42, then each step would theoretically be expected to be equal to 0.001 degree, and each 
element 42 would be 0.36 degrees or 360 steps from each adjacent element 42. However, in reality, there will 
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be deviations in the spacing of the elements 42. Therefore, the actual spacing between each adjacent pair of 
elements 42 is measured and can be stored in a calibration table, which table will remain accurate as long as 
the position sensing system of the present invention remains assembled. 

Initially, in calibrating the member 30, a first element 42 receives a signal from a reference retroreflective 
element, and this first element 42 becomes an index element for purposes of calibration of the member 30. 
The calibration system 44 determines when the index and following elements 42 are in position to commence 
or end the counting of the steps between elements 42 by employing the interpolation equation from above. For 
the index element 42, Tew equals zero and Tm equals zero. As the member 30 rotates toward the next adjacent 
element 42, Tm approaches zero and Tew approaches 1 . The moment in time when Tm is equal to zero and 
Tew is equal to one, then the element 42 is in position to end the counting of steps between the index element 
and the first adjacent element. This pattern continues until the number of steps and, thus, the distance in deg- 
rees between, each pair of adjacent elements 42 has been determined. The calibrating system 44 is associated 
with the computer 22 which includes a microprocessor, such as a Motorola 68030, having a memory means 
for providing storage of software and data. A listing of exemplary software for performing the calibration of the 
light transmitting and detecting means 14 which includes the member 30 of the present invention is as follows. 
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r i nc I ude 
line IucIg 
tiac lude 



<set jmp.h> 
<mat h . h> 
"cal. h " 



10 



15 



20 



25 



30 



40 



45 



50 



Idetine NxtA(x) 
Struct Sensor S(MAX_SENSOR+i J 



( ( x> =Nang le ) ?x-Nang I e+ 1 :x) 
{ 



o . 


-1 . 


-1 . 


1 , 


1302. 


6100 . 


2, 




2397 , 


3 , 


1077 . 


0, 


4 , 


3080 . 


44O0 , 


1 , 


0 . 


o. 


2. 


1000 , 


o. 


3 , 


2000 , 


o. 




3000 . 




s. 




-1 . 


6. 




-1 . 






-1 . 


8, 




-1 . 


a. 




-1 . 


10. 




-1 . 


11. 




-1. 


12. 




-1 . 


13. 




-1 . 


14, 




-1 . 


15. 




-1 . 


16. 




-1 



}; 

struct angle ALMAX_AN6LE3 ; 

Jmp^buf env_esc , envctrlc ; 

static 1nt SwFlags; 

1nt SwFlags, Ncalcu; 

1nt PTIcks, PFticks. PNcalcu; 

int Tsteps; 

int Gsteps, Gpatrn; 



float GetlangleQ; 

float x[MAX_ANGLE3 . y LMAX_AN6LE] ; 

1nt T-ImerO, LntrlndexC).. TraplSQ; 

char *range(); 
struct CurPos CP; 

char buffertOO], cmdLeo], lcrgsQ«w); 
int Stdln, Stdout; 
main( > 
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int koy; 



10 



20 



25 



30 



45 



I lcd("** CAL1 CAPliY mm ~)i 

set jm|>^ envctrlc ); 

gen_car>g'le(0> ; 
qen__coord(U) ; 



Stdin - 0; 

Stdout = U; 

Ticks » 0; PUcks =0; PKtlcks = U; 

PNcalcu = 0: Tsteps = U; base - 10: 

15 Gsteps = 0; (apatrn - 0; 

I2cJnit(); 

FifoResecO; 

__ Irap1( LVLb. Timer ); 

TrapU LVLS, lntr Index ); 

I rapl( Uxbc, 'frapIS ); 

Tnap0( 0x0200 ); 

If ( setjcnp( envesc ) ) ( 
EscControl ( ) ; 

> 

TtyUontrol ( ) ; 

> 

ItyControl ( ) 
{ 

register 1nt 1 ; 
register char c, crad ; 

prirvtf (*\n->; 
forC;;> { 

printf<*Cal(%d> i*„ Tsteps); 
getcmd( buffer, largs >; prlntf C"V*' ) ; 
cmd = buff or [0] ; 

sw1tch( cmd ) { 

case ' x 1 i 
case «X« i 
break; 

case ' f* s 
case * r ' i 



PR_cmd( cmd, largs ): 

break ; 

case 'x* : 

prlntf C"1 steps ff*d set to *, Tsteps ); 
Tsteps =0; 

j>r1ntf ( - %d A"" . T stops >; 
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case ' tz ' 
case ' C ' 

5 C_cmd( crtid, largs ); 

break ; 

case ' 1 ' : 
case * I ' 

l_cmd( ctnd, largs ); 

10 break; 

case ' t ' 
case * 1 ' : 

T_cmd( cmd, largs ); 

break; 

15 . . 

case S : 

iS_cmd( cmd, largs ); 

break; 

~ — case '0' : 

case ' F 1 ; 

0_cmd( crad, largs ); 
break ; 

case 1 n ' t 
case " N * = 

N_cmd( cmd, largs ); 

25 break; 



20 



case * G * : 

gfxca"l( Cangle ); 



V 

30 break; 



default - 

AE_cmd( cmd . largs > ; 

break; 



35 ) 

C_cmd( cmd, largs ) 
register char cmd, *1args; 

{ 

register 1nt i: 
40 register float *a ; 

float a co "I, acap, adif, onu \ ; 

Tsteps - 0} 

a ~ (float *)Uxl80U0; 

1f( cmd == *C» ) for( i=0; 1<1 000+1 ; i++ ) a{.1] = -l.U; 

« for(;;> { 

45 acal * lsteps/1000.0; 

acap = GetlangleC 1 - Si0 
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print f( "^;>td : V/ . J I ** - istnpa. acal ); 
it t ocap ! = - > . O J 1 

*1t <-:md - * (J 1 ) r2 acap; 

acap = roddcg(acap) ; 

it( ! Isteps ) anu I = acap: 

acap - = anu 1 ; 

if( acap * O.O ) acap += 3bU.U; 

adit = acap - acal; 

ppintt'(" - %7.3f = %7.3f\n* r acap. adit ); 

I 

else ppiatf (" angle not found ***\n"); 

i'f( (TctGps + 360) > 3600t)0 ) break; 
Xstep( 3U0 ) ; 
a++ ; 

> 

> 

l_crnd( cmd, large > 
register char cmd, *largs; 
{ 

register int i; 
register f loat *a ; 
float as, aD. al. adif; 

a = (float *)0k180U0; 

a1 * a[Oj; 

as = a CO] ; 

for( i=U ; K1000; 1++ ) ( 

aO = al; 

al = «Cil ; 

ifC a1 „ -i.o ) < 

pr1ntf(*%4d z Not Registered.^ • 1 ) : 

> 

adif = aO-al : 

printf<"%4d i %10-3f <%6.3f ) -> %TCKJf.", 

1 r raddeg(al) r raddeg(adi f ) , raddeg(as) ); 
adif = fabs(aO-al); 

as 3 al - a [0] ; 

1f( (raddeg(ad1f ) < 0.350) [| ( raddegCodi f)>D.3YU) ) i 
pp1ntf(Mi&lii&l'); 

> 

printf ("\n" ) ; 

) 

> 

float GetlangleC is, navorago > 

register int ia, naverage; 

( 

float atot - U.0; 

register int i r nfound. ntries. na ; 

nfound - 0; 
ntries — 0; 

for(;;) { 
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no •■= CopAng lei>( ) ; 

tOI'( 1 -O ; 1<Od; ) { 

i + C copA(i j .iseivjor != i a 
a tot •»-- copA[i] .a; 
n found** ; 
break ; 

> 

if^ nfound = = naverage ) ( 

return( atot/naverage ) ; 

> 

ift ++ntries > 2*naverage ) 
return ( -1.0 ) ; 



EscContro I ( ) 

c 

register char d, c2 ; 
register 1nt xstep; 

_ c1 - getch() ; c2 = getch(); 

if( c2 == -0* > xstep = -1 
else if( c2 — 'C ) xstep = 1 

else xstep = 0 

Xstep( xstep ); 

1 

M_ cmd( end r large ) 
register char cmd, *1args; 
I 

register 1nt i 

register float *al = (float *)0x1S000; 

1fC cmd -= * N * ) { 

for( 1=0; KNCW+1; 1++ ) Canglef* 
printf(*Ca1 copied to Cangle.\n" ) 

> 

i f ( cmd == ' n ' ) { 

gen_cangle( 1 ) ; 

printf ("Canglo initial 1sed ) ; 

> 



O^crod ( cmd, largs ) 
register char cmd, 'Margs; 
( 

register int 1 , icw; 

float a. aO, al . temp; 

float step - M_2P1 / 1000.0; 

register float *ap = (float *)0x18U00; 

register float *a1 ~ (float *)Ux19t)00; 

for( 1=0; 1OUU0+1; 1+4- ) a 1 1 1 ] = -1-0: 
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for( i<1UOU+l ; i*. ) { 

aO - ,iV I i - 1 J ; 

ol = apt i ] ; 

if ( {i'.a -= -1.0) || (el -1.0) > continiiB; 

temp = al/stup - O.S; 

lew = temp; 

a = icw * step; 

ii'lficwj = (n-1) * step + C (a-aaj/(a1-al))) * step; 
temp = al [icw] ; 

if(tstqual('p')) printtC%4d: %tt . 3f ,%B . 3t %4d %B.3f %U.3f\n". 

1 . raddeg(aO) „ raddegU 1 ).icw, raddcg(a) „ raddeg( temp) ) ; 



c teon tab ( a 1 ) ; 

if< cmd = - 1 P ' ) ( 

printf ("\nf loat Cang le C 1 000 + 1 3 - {"); 
for( i=U; K1000+1; i*+ ) { 

if( !<1*B) ) pr1ntf('\n")j 
temp s a 1 1 1 J ; 

ifC "I != 1000 ) printf C"*a. 61 s . " , temp ); 
else printf (•%«*. 6f * , temp ); 

> 

printf ( "\n) ; \n" ) ; 

} 

e'lee { 

printf ( * \nCa 1 1 brat ion i"); 
for( 1=0; K1000+1 ; 1++ ) < 

if< !C1%5) ) pr1ntf(-\n %10.3f (f loat ) (1*0 . 36D )) ; 

temp = raddeg(Cang le[1 3) - raddeg(aUO); 

-ff( i != 1000 ) printf ('%9.3f .* . temp ); 

else printf ("%9.3f temp ); 

> 

printf C\n) ;\n- ) ; 

} 

> 

clean tabC Ca ) 

float~CaU;* 

( 

int nclns ; 

float temp, low^a , h1gh_a. d1f_a; 
register int low^icw. hiqh_icw. n^icw. i ; 



low_lcw 5= 0; 

wMle( Co[low_icw"j »- -1.0 ) low_1cw - nxticw( lowjcw); 

for(;;) { 

ndns = U; 

whi1e( Ca LnxticwC 1ow_Scw)'J ! = -1.0 > { 
)ow_1cw - nxClcw(1ow_1cw); 
Iow a « Ca O ow.icw'j ; 
IfC *-+nclns > 1000 ) { 

printt'C" Table cleaned . \n" ) ; 

goto CLEANED; 

1 
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if( tstquo I ( ' p * ) ) printt(" low_icw=!feci. low_a 
law icw , raddeg( low_a) ); 



it 1 U . 3f\r\- 



10 



15 



20 



high_icw= nxticw( low_icw), 

whi le( Cathigh icwJ - = -1.0 ) hiqh_1cw= nxcicw(high_icw) 
high_a = Co [high_icw j ; 

1f( tstquaK'p') )printf("hl g h_1cw^d r h1flh_a = %10.3f\n 
high_icw, raddeg(h1gh_e) ); 

d i f _ a = hi gh_a - 1ow_a; 
njcw hi g K_icw - "low_icw; 

1f( njcw < 0 ) n icw += 1UUC; 

if( tstqualf'p') ) pri nt f ( *d1 f _a=%1 U . 3f , n_1cw=%d\n 

raddeg(d1f _a) , n_icw ); 

dit : _a / = n_1cwj 

for( i=0; 1<=njcw; 1++ ) { 

temp = low_a + i * dif^a; 

Caf low icwj = te«np; 

1f( tstqualC'p') )printf C"%4d : %1-0.3'f\n'. 

low_icw , raddeg(temp) ) ; 
low -few = nxticw(low icw) ; 

} 



CLEANED z 
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35 



40 



45 



temp = Ca[C] ; 

t"or( 1=0; i<NCW; 1++ ) { 

Ca [i J temp; 

if( Ca[i] < 0.0 ) 

> 

CaCTOOOl * CaCO] : 



Ca[i] M_2f>L; 



static nxticw( lew > 
register 1nt lew ; 

if( ++icw > 993 ) icw = U; 

return C icw ) ; 

> 

FR_cmd( cmd r "largs ) 
register char cmd f ^1 args ; 
< 

int nsceps; 

if( largs ) rango( largs, fcnsteps, insteps >? 

else nsteps = I; 

printf C n%d steps %s.\n*. 

nsteps. (cmd== • f • )?* forwards* r * reverse* ) ; 



( cmd == *f ' ) 
1f< cmd == ' r ' ) 



Xstep( nsteps ) ; 
Kstep( -nsteps ) ; 



50 



55 



30 



EP 0 468 677 A2 



S cmd C cmd, largis ) 
register char cmd, *largs: 
I 

»nt ns ; 

int StbOOO] : 

int icw. stime, ctime; 

register int i „ J, imin, imax, hmax; 

it( *1orgs ) range( largs. ins, &ns ); 

else ns « 1000; 

tor( t = i<S0O0; i ) S[i3 = 0; 

pr int1 i ("»samp^es:%4d *, ns ); 

imin = 5 0 00; i max = 0; hmax - 0; 
forC Kns; 1++ ) { 

GctFTransC &icw, &stime, &ct1me ); 

if( stime < 1m1n ) imin = stlme; 

1f ( stimo > imax ) imax = stime; 

S[st:ime]++; 

1f ( S[stime] > hmax ) hmax = S[stimej 

) 

prlntfC 1rain=%4d imax»%4d hmax^d\n\ imin 
forC i<5000; i ++ > { 

if( SL"»3 " n > continue; 
forC J-i; J<SO00; ) { 

1fC SLJ3 " 0 ) return; 
prlntf C%4di%4d ■ . J, SCJ3 
p1ot_bar( 60 * SCJ]/ hmax ); 
printf ("\n*) ; 

> 



p"lot__bar< nstars > 
register int nstars; 

char bar£80l ; 
register char *b; 
register int i ; 

b = bar; 

whHe( nstars — > 0 > *b++ - 
*b = O; 

pr1ntfC%s - . bar ); 

> 

l_cmd( cmd, largs ) 
register char cmd, *largs; 
i 

float aO, al ; 

int n steps , n times ; 

ifC largs ) ( 

cmd « *1argc; 
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largs - rarge( larg;;, Snctcps. fcnstcps ); 

i 

c* \ vg nsteps = 1 ; 

it^ *l<Jr-gs ) largs = reir»ge( largs, Smtimes, intimes ); 

else n 1 1 mes = - 1 ; 



do I 



if( cmd •=•-'){ 

d() = Getlangle( 1, SO ) ; 

printf( - %d times *%d steps forwards ( %U . 3 1= )..."* . 

ntlmes, nsteps. Poddeg(a«) ) 

Xstep( nsteps ); 

al - GetLangle( 1. SO ); 

printf("and peverse(%8 . 3f ) .\n ' . raddeg(al)); 
Xstop( -nsteps ); 



> 

Q I SO { 



aO - Getlang'ie( 1 , 50 ) : 

pr1ntf<"%d t-fmes »*d steps r everse(%8 . 3f ) . . - * r 

ntimes. nsteps, raddeg(aU) ) 

Xstep( -nsteps ); 

al » GetlangleC 1 . 50 ) ; 

printfCand f orwards(%8 . 3f ) . \n* , raddeg(al) ); 
XstepC nsteps ); 



} 

} 

whi"!e( — ntlmes ); 



<step( nsteps ) 
register int nsteps; 
{ 

whileC Gsteps ): 

IfC nstops>0 ) Gpatpo = FWO; 
else tipatrn = REV; 

Gsteps - abs(nsteps); 

while( G«tops ); 
Gpatrn - 0; 

> 

micro 

register struct pit *p = PIT; 

p->pbcr = 0x80 
p->pbddr= OKlf 
p->pbdr = 0x00 



IntrT1cks() 
{ 

register 1nt t; 

register struct pit *p - fMT: 
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T i cks + + ; 

if( i(Ticks%3) && Gsteps ) { 
5 p->pbdr = Gpatrn; 

Gsteps ; 

•*f< Gpatrn — FWO ) Tsteps++; 
else T steps — ; 

} 

else p-> pb 



The position sensing system which combines the use of the code wheel 30 and the motor 26 is supported 
by a hardware interface 52, illustrated in Fig. 5. An event occurs every time an aperture 42 on the code wheel 

15 30 passes, or a retroreflective element 1 8 commences or ends a reflection of the beam of light 16. Due to the 
high precision time measurements required between each adjacent pair of apertures 42, a reference clock 54 
is used in keeping a record of an event. If an event occurred during this time, it is stored in a circuit 56, such 
as a 32 bit first-in-first-out circuit. The circuit 56 records the movement of the code wheel 30 at register 0. The 
actual element or aperture 42 which is currently passing is sensed at a member or code wheel pick up element 

20 58 and counted by a member rotation counter 60. Each time the member 30 has completed a full rotation, an 
index pick up element 62 sends a signal to reset the member rotation counter 60. The member pick up element 
58 and the index pick up element 62 comprise means for detecting movement of each of the elements 42 past 
a predetermined point as the member 30 rotates. In a preferred embodiment of the present invention, the pick 
up elements 58 and 62 comprise a light source paired with a photodetector element 

25 The light transmitting and detecting means 14 may include means for diverting and focusing the returning 

beam to the photodetector 36. The rotating mirror 32 diverts the light beam 16 toward a collimating lens 40, 
which lens 40 focuses the light beam 16 toward the photodetector 36. Signal processing means 38 detect when 
receiving optics 64, consisting of collimating lens 40 and photodetector 36, is either commencing receipt of the 
reflection of the light beam 16 or ending receipt of the reflection of the light beam 16 from the retroreflective 

30 elements 1 8 to the light transmitting and detecting means 14 of Fig. 2. Signal processing means 38 can trans- 
form the analog signal 41 from photodetector 36 into two digital signals which are received at register 1 of circuit 
56. The firstdigital signal represents a START signal which indicates if the reflection from retroreflective element 
18 is commencing the reflection of beam 16 from the retroreflective elements 18 to the light transmitting and 
detecting means 14, and the second digital signal is an END signal which indicates if the reflection is ending. 

35 Register 2 receives signals for measuring the time elapsing between the passage of the last aperture 42 and 
an event, which event may be the time Tm or the time Tew shown in Fig. 3. A clock pulse counter 66 is reset 
at a clock pulse for counter 60 for each aperture 42, which counter 66 counts the time elapsing between the 
passage of each pair of adjacent elements 42. Information regarding the capacity of the circuit 56 is stored in 
register 3. 

40 The circuit 56 stores the information received and provides an output signal 68 to the computer 22 which 

includes a microprocessor having memory means. The computer 22 is responsive to the output signal 68, to 
compute the coordinates of the position of the point 12 in the frame of reference 1 0. When the light transmitting 
and detecting means 14 is positioned at point 12, the computer 22 also computes the orientation of the light 
transmitting and detecting means 14 within the frame of reference 10. 

45 Fig. 5 also illustrates the variable zenith angle of the beam which results from the tilt of any of the mirrors 

32, 35, and 37 of Fig. 2. Varying the zenith angle of the beam 16 allows the position sensing system to scan 
an environment 1 0 and find all of the retroreflective elements 18 even when the elements 1 8 are not positioned 
at equal elevations. Referring back to Fig. 1, the dynamic scan pattern 17 can be tilted up or down as the beam 
16 approaches a retroreflective element 18 to permit the beam 16 to strike the retroreflective element 18. 

so Referring now to Figs. 6A and 6B, a flow chart illustrating the steps for positioning or tilting a mirror and 

providing feedback regarding the Z position of the point 12 of Fig. 1 is shown. Mirror 32 is used here by way 
of example and is not intended to be limiting, as any or all of the mirrors 32, 35, and 37 may be tilted to vary 
the zenith angle of the beam 16. This information is then used for computing the X-Y-Z coordinates of the point 
12. The flow chart is triggered in Fig. 6A by the main routine software at step 70. Next, steps 72 and 74 require 

55 tilting the mirror 32 of Fig. 2 to its upper limit and acquiring data relating to the position of the mirror 32 as the 
shaft 28 rotates the mirror 32 through one rotation. At step 76, the program determines which, if any, reflectors 
were found during the rotation of the mirror 32 as the mirror 32 was tilted to its upper limit. The exemplary 
software for performing angle measurements and bar code recognition may be applied to the program at this 
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point. 

Continuing with Fig. 6A, decision block 78 determines whether the mirror has been tilted sufficiently to reach 
its lower limit of range. If not, the mirror 32 is tilted slightly at block 80. If the mirror 32 has been tilted through 
its full range, the current X-Y position of the point 1 2 is computed at block 82. The exemplary software for per- 
forming the triangulation calculations may be applied here before continuing to step 84 where the elevation of 
the beam 16 striking the retroreflective elements 18 is calculated. The elevation can be determined using the 
known retroreflective element locations and the dark/light ratio reflection from the distinctive slant targets 
extending substantially diagonally from one top corner to the opposing bottom comer of the retroreflective ele- 
ments 18 in combination with a retroreflective barcode. 

The program continues to step 86 where an equation for the plane which intersects the beam 16 is gen- 
erated. At step 88, then, the program calculates the elevation of the plane at the current X-Y position computed 
at step 82. Using the elevation of the plane at the current X-Y position and the known tilt angle of the mirror 32 
when the beam 16 strikes the retroreflective elements 18, a Z position is calculated at step 90. Then using the 
coordinates from previous rotations from steps 82 and 90, the mirror is tilted into the correct position for striking 
the next retroreflective element 18 at step 92. 

At step 94, using the exemplary angle measurement software and bar code recognition software, the bar 
code and angle of the next retroreflective element to be struck by the beam 1 6 are read. Decision block 96 of 
Fig. 6B then determines if a full rotation has occurred since the last position calculation. If not, the program 
returns to step 92. If so, the program continues to decision block 98 to determine if at least three retroreflective 
elements 18 were found. If so, the program returns to step 82. If not, the program continues to block 100 to 
calculate the probable new coordinates of the point 12 and establishes directions to the missing retroreflective 
elements 18, using coordinates of the point 12 from previous rotations. 

Once the directions to the missing retroreflective elements 18 have been calculated, the program continues 
to step 1 02 where the mirror 32 is tilted to scan slightly above the probable position of the missing retroreflective 
elements 18. Data is then acquired for one rotation of the mirror 32 at step 104, after which the program deter- 
mines at step 106 which, if any, of the missing retroreflective elements 18 were found. If the missing retroref- 
lective elements 1 8 were found, decision block 1 08 directs the program to step 82. If the missing retroreflective 
elements 18 were not found, the program continues to decision block 110 to determine if the mirror 32 was 
tilted to a position slightly below the missing retroreflective elements 18. If so, the program returns to step 72 
and repeats. If not, the program continues to step 112 where the mirror 32 is tilted slightly before returning to 
step 104 and again acquiring data for one rotation of the mirror. Using the steps detailed in the flow chart of 
Figs 6A and 6B, the movement and position of the mirror 32 can be determined and applied to the X-Y-Z cal- 
culation of the point 12. 

Having described the invention in detail and by way of reference to preferred embodiments thereof, it will 
be apparent that modifications and variations are possible without departing from the scope of the invention 
as defined in the appended claims. 

Claims 

1. A system for determining the position of a point (12) in three dimensional space, the system comprising: 

at least three stationary retroreflective elements (18) spaced apart from each other and stationed 
at known coordinates, said retroreflective elements (18) capable of reflecting light back toward a light 
source (20); 

light transmitting and detecting means (14), positionable at the point (12), for generating a rotating 
beam of light (16) to illuminate said stationary retroreflective elements (1 8) during rotation, and for detect- 
ing said beam of light (16) when it is reflected from said stationary retroreflective elements (18) and 
generating an output signal in response thereto; and 

computer means (22) responsive to said output signal for computing from said known coordinates 
of said retroreflective elements (1 8) and from the angular orientation of said beam (1 6) when said beam 
(16) illuminates said retroreflective elements (18), the X-Y-Z coordinates of the position of the point (12) 
in space. 

2. A system for determining the position of a point (1 2) as claimed in claim 1 , characterized in that said com- 
puter means (22) are capable of determining an orientation of said light transmitting and detecting means 
(14) when said light transmitting and detecting means (14) are positioned at the point (12). 

3. A system for determining the position of a point (12) as claimed in claim 1 or 2, characterized in that said 
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at least three stationary retroreflective elements (18) have a distinctive retroreflective slant target. 

4. A system for determining the position of a point (12) as claimed in claim 1 or 2, characterized in that said 
at least three retroreflective elements (18) have a distinctive combination of a retroreflective slant target 

5 extending substantially diagonally across each of said at lease three retroreflective elements (18) and a 

retroreflective bar code. 

5. A system for determining the position of a point (12) in three dimensional space, the system comprising: 

at least three stationary retroreflective elements (18) spaced apart from each other and stationed 
10 at known coordinates, said retroreflective elements (18) capable of reflecting light back toward a light 

source (20); 

light transmitting and detecting means (14), positionable at the point (12), for generating a rotating 
beam of light (16) to illuminate said stationary retroreflective elements (1 8) during rotation, and for detect- 
ing said beam of light (16) when it is reflected from said stationary retroreflective elements (18) and 
15 generating an output signal in response thereto, said light transmitting and detecting means (14) including 

a member (30) rotating with said beam (1 6), said member (30) having a periphery and further 
having a plurality of angularly positioned elements (42) spaced around said periphery which divide a 
revolution of said member (30) into a plurality of generally equal partial revolutions, and 

a means for detecting movement of each of said elements (42) past a predetermined point 
20 as said member (30) rotates; and 

computer means (22), responsive to said output signal and said means for detecting move- 
ment of each of said elements (42) past a predetermined point, for computing the X-Y-Z coordinates of 
the position of the point (12) in space. 

25 6. A system for determining the position of a point (12) as claimed in claim 5, characterized in that a calibrating 
system (44) is provided for calibrating said light transmitting and detecting means (14). 

7. A system for determining the position of a point (12) as claimed in claim 6, characterized in that said calib- 
rating system (44) comprises a device having an angularly positioned surface for arbitrarily positioning 
30 said light transmitting and detecting means (14), wherein said light transmitting and detecting means (14) 

include said member (30) rotating with said beam (16); and means for measuring a distance between a 
first said angularly positioned element (42a) and an adjacent second said angularly positioned element 
(42b). 

35 8. A system for determining the position of a point (12) as claimed in claim 5, 6 or 7, characterized in that 
said plurality of angularly positioned elements (42) comprise a plurality of apertures. 

9. A system for determining the position of a point (12) as claimed in claim 5, 6 or 7, characterized in that 
said means for detecting movement of each of said elements (42) comprises a light source paired with a 

40 photodetector (36) element. 

10. A system for determining the position of a point (12) in space, the system comprising: 

at least three stationary retroreflective elements (18) spaced apart from each other and stationed 
at known coordinates, said retroreflective elements (18) capable of reflecting light back toward a light 
45 source (20); 

light transmitting and detecting means (14) positionable at the point for generating a rotating beam 
of light (16) to illuminate said stationary retroreflective elements (18) during rotation and for receiving said 
beam of light (16) reflected from said stationary retroreflective elements (18) when said beam (16) illumi- 
nates said retroreflective elements (18), said light transmitting and detecting means (14) including 
so an optical member (30) rotating with said beam (16), said member (30) having a periphery 

and further having a plurality of apertures (42) located at said periphery which divide a revolution of said 
member (30) into a plurality of generally equal partial revolutions, 

a means for registering a time differential between each adjacent pair of said plurality of aper- 
tures (42) as said member (30) rotates, 
55 a means for registering a tilt position of one or more reflecting mirrors positioned at said light 

transmitting and detecting means (14), and 

a means for calculating a zenith angle of said beam based on said mirror position (32); and 
computer means (22), responsive to said output signal and said means for detecting move- 
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ment of each of said elements (1 8) past a predetermined point, for computing the coordinates of the posi- 
tion of the point (12) in space and for computing an orientation for said light transmitting and detecting 
means (14) when said light transmitting and detecting (14) means are positioned at said point (12). 

11. A method for determining the position of a point (12) in three dimensional space, the method comprising 
the steps of: 

locating at least three stationary retroreflective elements (18) spaced apart from each other and 
stationed at known coordinates, said retroreflective elements (18) capable of reflecting light back toward 
a light source (20); 

transmitting a rotating beam of light (16) to illuminate said stationary retroreflective elements (1 8) 
during rotation; 

measuring an angular position of said retroreflective elements (18) with respect to a known refer- 
ence; 

tracking scan behavior; 

using said scan behavior to calculate angular measurements; and 

determining the three dimensional position of the point (12) dimensional based on said angular 
measurements and said coordinates of said retroreflective elements (18). 

12. A method for determining the position of a point (12) as claimed in claim 11, characterized in that said 
step of tracking scan behavior further includes the steps of mounting a member (30) on a rotating shaft, 
said member (30) having a periphery and further having a plurality of apertures (42) located at said 
periphery which divide a revolution of said member (30) into a plurality of generally equal partial revolu- 
tions; and registering a time differential between each adjacent pair of said plurality of apertures (42) as 
said member (30) rotates 

13. A method for determining the position of a point (12) as claimed in claim 11 or 12, characterized in that 
said step of determining the position of the point(12) further includes the step of determining an orientation 
for said rotating beam of light (16) when said rotating beam of light (16) is positioned at the point (12). 

14. A method for determining the position of a point (12) as claimed in claim 11, 12 or 13, further including 
the step of interpolating between corresponding angle measurements of an adjacent pair of said plurality 
of apertures (42) to determine exact angles between each said pair of apertures (42). 
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MEANS 
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FIG -3 
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FIG-6A 
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FIG-6B 
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